/*
 * main.cpp
 *
 * Author: Kellen Carey
 *   Date: June 20, 2014
 */
#include "Vision.h"

int main(int argc, char** argv) {
	MotorController motorController;
	Vision vis(motorController);
	std::cout << "Created Vision Instance" << std::endl;
	bool position = false;
	bool done = false;

	vis.setAction(SEARCH_FOR_GOAL);
	//	vis.setAction(CENTER_BALL);

	while (!done) {
		if (!position && vis.knowsRobotPosition()) {
			vis.setAction(SEARCH_FOR_BALL);
			std::cout << "Searching for Ball" << std::endl;
			position = true;
		}
		else if (vis.knowsBallPosition()) {
			vis.setAction(SEARCH_FOR_BOTH);
			std::cout << "Searching for both" << std::endl;
			done = true;
		}

		int c = waitKey(1);

		if ((char)c == 27) {
			done = true;
		}

		vis.nextFrame();
	}
	vis.setAction(WAIT);
	//	vis.setAction(SEARCH_FOR_BOTH);
	// Robot knows where both the ball and the field are
	std::cout << "Vision knows where it is, and where the ball is.\n" <<
			"We would now normally wait for the robot to move," <<
			" and then update our world." << std::endl;
	std::cout << std::endl;
	std::cout << "The robot is located: (" << vis.getRobotX() <<
			", " << vis.getRobotY() << ") @ " << vis.getRobotTheta()
			<< std::endl;
	std::cout << "The ball is located: (" << vis.getBallX() << ", "
			<< vis.getBallY() << ")" << std::endl;

	done = false;

	// TODO I forgot how the robot's X and Y are oriented, might have to change some vision code if this doesn't work
	while (!done) {
		vis.setAction(CENTER_BALL);
		vis.nextFrame();
		int previousRobotX = vis.getRobotX();
		int previousRobotY = vis.getRobotY();
		double robotTheta = vis.getRobotTheta();

		int ballX = vis.getBallX();
		int ballY = vis.getBallY();

		double turningTheta = motorController.getHeadAngle();
		std::cout << "Turning theta: " << turningTheta << std::endl;

		// TODO We assume here that each turn left motion turns us approximately 45 degrees
		if (turningTheta > 23) {
			//			if (motorController.step(false)) {
			//				motorController.setMotion("Turn Left");
			vis.updateRobotTheta(45);
			std::cout << "Robot turning left. New theta = " << vis.getRobotTheta() << std::endl;
			//			}
		}
		// TODO We assume here that each turn right motion turns us approximately 45 degrees
		else if (turningTheta < -23) {
			//			if (motorController.step(false)) {
			//				motorController.setMotion("Turn Right");
			vis.updateRobotTheta(-45);
			std::cout << "Robot turning right. New theta = " << vis.getRobotTheta() << std::endl;
			//			}
		}
		// Here we assume that we are facing in the correct direction, so let's walk forward
		else if (vis.getBallDistance() > 100) {
			//			if (motorController.step(false)) {
			//				motorController.setMotion("Walk Forward");
			std::cout << "Robot walking forward. New coordinates: (" << vis.getRobotX() << ", " << vis.getRobotY() << ")" << std::endl;

			// TODO We assume that the robot moves 3 cm every time it takes a step
			vis.updateRobotPosition(3, robotTheta);
			//			}
		}
		else {
			done = true;
		}
	}
	return 0;
}
